/*
 * Link.h
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */


#ifndef LINK_H_
#define LINK_H_

#include "Matrix.h"

#define PI 3.1415926535897932384626433832795
#define DEG2RAD (PI/180)
#define RAD2DEG (180/PI)

class Link {
public:
	Link() {
		length=0;
		mass=0;
		q=0;
		dq=0;
		ddq=0;
		q_old=0;
		dq_old=0;
		ddq_old=0;
		tau=0;
		tipx=0;
		tipy=0;
		tipz=0;
		CGtip_x=0;
		CGtip_y=0;
		CGtip_z=0;
	}

	virtual ~Link() {
	}

	Matrix getCG() const { return CG; }
	double getCGtip_x() const { return CGtip_x; }
	double getCGtip_y() const { return CGtip_y; }
	double getCGtip_z() const { return CGtip_z; }
	Matrix getDH() const { return DH; }
	double getDdq() const { return ddq; }
	double getDdq_old() const { return ddq_old; }
	double getDq() const { return dq; }
	double getDq_old() const { return dq_old; }
	double getLength() const { return length; }
	double getMass() const { return mass; }
	double getOld() const { return q_old; }
	double getQ() const { return q; }
	double getTau() const { return tau; }
	double getTipx() const { return tipx; }
	double getTipy() const { return tipy; }
	double getTipz() const  { return tipz; }

	void printInfo() {
		cout << "CG: " << CG;
		cout << "DH: " << DH;
		cout << "Length: " << length << endl;
		cout << "Mass: " << mass << endl;
		cout << "Angle: " << q << endl;
	}

	void setCG(Matrix &newCG) {
		CG.resize(newCG.getX(), newCG.getY());
		CG=newCG;
	}
	void setCGtip_x(double CGtip_x) { this->CGtip_x = CGtip_x; }
	void setCGtip_y(double CGtip_y) { this->CGtip_y = CGtip_y; }
	void setCGtip_z(double CGtip_z) { this->CGtip_z = CGtip_z; }
	void setDH(Matrix &newDH) { 
		DH.resize(newDH.getX(), newDH.getY());
    		DH = newDH;
	}
	void setDdq(double ddq) { this->ddq = ddq; }
	void setDdq_old(double ddq_old) { this->ddq_old = ddq_old; }
	void setDq(double dq) { this->dq = dq; }
	void setDq_old(double dq_old) { this->dq_old = dq_old; }
	void setLength(double length) { this->length = length; }
	void setMass(double mass) { this->mass = mass; }
	void setOld(double q_old) { this->q_old = q_old; }
	void setQ(double q) { this->q = q; }
	void setTau(double tau) { this->tau = tau; }
	void setTipx(double tipx) { this->tipx = tipx; }
	void setTipy(double tipy) { this->tipy = tipy; }
	void setTipz(double tipz) { this->tipz = tipz; }

	Matrix forwardK() {
		Matrix A = DH.transMatrix();
		//cout << "LINK: " << A  << endl;
		return A;
	}

private:
	double length;
	double mass;
	Matrix DH;
	Matrix CG;
	double q;
	double dq;
	double ddq;
	double q_old;
	double dq_old;
	double ddq_old;
	double tau;
	double tipx;
	double tipy;
	double tipz;
	double CGtip_x;
	double CGtip_y;
	double CGtip_z;
};

#endif /* LINK_H_ */
